/*
 * @Author: lvgrape lvgrape@outlook.com
 * @Date: 2024-06-19 10:18:46
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2024-07-08 18:00:22
 * @FilePath: \zino_premium_rt\ZINO\acceleration\acceleration.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <stdint.h>
#include "acceleration.h"
#include "zino.h"
#include "lsm6dsr_spi2.h"
#include "lsm6dsr_reg.h"

// #include "shanwai.h"
#include "rgb_tmr.h"

#define DBG_TAG "zAccel"
#define DBG_LVL DBG_LOG
#define LSM6DSR_USE_SPI
// #define LSM6DSR_USE_I2C
#include <rtdbg.h>

#include "zino_config.h"

struct acc_offset
{
    uint8_t flag;
    i16_vector_t offset;
    i16_vector_t level_offset;
};
ZINO_CONFIG_DECLARE(struct acc_offset, accOffset);
ZINO_CONFIG_REGISTER(struct acc_offset, accOffset, ZINO_CONFIG_IDX_ACC_6FACE_CALIBRATION, 1);

const int8_t face_vector[6][3] = {
    {1, 0, 0},  // 5,x+
    {-1, 0, 0}, // 6,x-
    {0, 1, 0},  // 3,y+
    {0, -1, 0}, // 4,y-
    {0, 0, 1},  // 1,z+
    {0, 0, -1}, // 2,z-

};
void zino_accel_6face_calibrate_start(struct acc_device *dev)
{
    dev->calibrated_face = 0;
}

static int zino_acc_init(struct acc_device *dev)
{
    dev->user_data = dev;
    dev->scale_factor = lsm6dsr_from_fs2g_to_mg(1)*0.001f; // scale at 1
    /**
     * @brief lsm6dsr accel adc range:
     * 2G:  -16384 ~  +16384
     * 4G:  -8192  ~  +8192
     * 8G:  -4096  ~  +4096
     * 16G: -2048  ~  +2048
     *
     */
    dev->acc_1G = 16384;
    dev->variance.buffer = dev->traceBuffer;
    dev->variance.len = ACC_CALIBRATION_COUNT;
    dev->variance.variance = 0;
    dev->sample_count = 0;
    dev->cali_face = 2;
    dev->varinace_limit = 500;

    if (accOffset()->flag == 0xcc)
    {
        dev->offset.x = accOffset()->offset.x;
        dev->offset.y = accOffset()->offset.y;
        dev->offset.z = accOffset()->offset.z;
        dev->calibrated_face = 0x3f;
        LOG_D("loading: Acceleration offset: %d, %d, %d", dev->offset.x, dev->offset.y, dev->offset.z);
    }
    else
    {
        zino_accel_6face_calibrate_start(dev);
    }

    return lsm6dsr_spi2_read_data_polling_int();
}
static int zino_get_raw_data(struct acc_device *dev)
{
    int ret = 0;
    dev->adc.x = 0;
    int16_t buff[3];
    ret = lsm6dsr_spi2_read_accel_adc_data(buff);
    dev->adc.y = -buff[0];
    dev->adc.x = -buff[1];
    dev->adc.z = -buff[2];
    return ret;
}


int isAroundFace(struct acc_device *dev, uint8_t face)
{
    const int16_t limit = dev->acc_1G * 5 / 100; // 5%
    face -= 1;
    uint8_t x = dev->adc.x < (face_vector[face][0] * dev->acc_1G + limit) && dev->adc.x > (face_vector[face][0] * dev->acc_1G - limit);
    uint8_t y = dev->adc.y < (face_vector[face][1] * dev->acc_1G + limit) && dev->adc.y > (face_vector[face][1] * dev->acc_1G - limit);
    uint8_t z = dev->adc.z < (face_vector[face][2] * dev->acc_1G + limit) && dev->adc.z > (face_vector[face][2] * dev->acc_1G - limit);
    // LOG_D("face:%d, limit:%d %d,%d,%d %d.%d.%d %d", face, limit, dev->adc.x, dev->adc.y, dev->adc.z, x, y, z,face_vector[face][2] * (dev->acc_1G + limit));
    if (x && y && z)
        return 1;
    return 0;
}
uint8_t get_face_from_vector(struct acc_device *dev)
{
    uint8_t face = 0;
    for (int i = 1; i <= 6; i++)
    {
        if (isAroundFace(dev, i))
        {
            face = i;
            return face;
        }
    }
    return 0;
}
#define RGB_CNT_MAX 100
void face_on_calibration_rgb_state(struct acc_device *dev, uint8_t face)
{
    const uint8_t colorSet[3][3] = {
        {255, 0, 0},
        {0, 255, 0},
        {0, 0, 255},
    };
    uint8_t r, g, b;
    uint8_t fcnt = face >> 1;

    if (dev->calibrated_face & (1 << face))
    {
        if (dev->onCaliRgbCnt < RGB_CNT_MAX / 2)
        {
            r = 0;
            g = 0;
            b = 0;
        }
        else
        {
            r = colorSet[fcnt][0];
            g = colorSet[fcnt][1];
            b = colorSet[fcnt][2];
        }
    }
    else
    {
        if (fcnt % 2)
        {
            r = colorSet[fcnt][0] * dev->onCaliRgbCnt / RGB_CNT_MAX;
            g = colorSet[fcnt][1] * dev->onCaliRgbCnt / RGB_CNT_MAX;
            b = colorSet[fcnt][2] * dev->onCaliRgbCnt / RGB_CNT_MAX;
        }
        else
        {
            r = colorSet[fcnt][0] * (RGB_CNT_MAX - dev->onCaliRgbCnt) / RGB_CNT_MAX;
            g = colorSet[fcnt][1] * (RGB_CNT_MAX - dev->onCaliRgbCnt) / RGB_CNT_MAX;
            b = colorSet[fcnt][2] * (RGB_CNT_MAX - dev->onCaliRgbCnt) / RGB_CNT_MAX;
        }
    }
    // LOG_D("face:%d/%d, r:%d, g:%d, b:%d", face,fcnt, r, g, b);
    rbg_led_set(r, g, b);
    if (dev->onCaliRgbCnt++ >= RGB_CNT_MAX)
        dev->onCaliRgbCnt = 0;
}
void zino_calibrate_face(struct acc_device *dev, uint8_t face)
{
    face -= 1;
    face_on_calibration_rgb_state(dev, face);
    int16_t var = variance_calculate(&dev->variance, dev->adc.raw[face / 2]);
    if (var < dev->varinace_limit)
    {
        if (dev->sample_count < ACC_CALIBRATION_COUNT*10)
        {
            dev->sample_count++;
        }
        else
        {
            dev->calibrated_face |= (1 << face);
            dev->sample_count = 0;
            if (face % 2 == 1)
                dev->down.raw[face / 2] = dev->variance.average;
            else
                dev->up.raw[face / 2] = dev->variance.average;
        }
    }
    else
    {
        LOG_D("face:%d, variance:%d ave:%d", face, var,dev->variance.average);
        dev->sample_count = 0;
    }
}

static int zino_acc_perform_6_face_calibration(struct acc_device *dev)
{
    if (dev->calibrated_face == 0x3F)
    {
        // LOG_D("calibrated_face:%d",dev->calibrated_face);
        // LOG_D("Up:%d,%d,%d",dev->up.raw[0],dev->up.raw[1],dev->up.raw[2]);
        // LOG_D("Down:%d,%d,%d",dev->down.raw[0],dev->down.raw[1],dev->down.raw[2]);
        return 0;
    }
    uint8_t face = get_face_from_vector(dev);
    if (face != 0)
    {
        zino_calibrate_face(dev, face);
        if (dev->calibrated_face == 0x3F)
        {
            dev->offset.x = dev->up.x + dev->down.x;
            dev->offset.y = dev->up.y + dev->down.y;
            dev->offset.z = dev->up.z + dev->down.z;

            accOffset()->flag = 0xcc;
            accOffset()->offset.x = dev->offset.x;
            accOffset()->offset.y = dev->offset.y;
            accOffset()->offset.z = dev->offset.z;

            zino_config_save_all();
            LOG_D("calibrated_face:%d", dev->calibrated_face);
            LOG_D("Up:%d,%d,%d", dev->up.raw[0], dev->up.raw[1], dev->up.raw[2]);
            LOG_D("Down:%d,%d,%d", dev->down.raw[0], dev->down.raw[1], dev->down.raw[2]);
            LOG_D("Offset:%d,%d,%d", dev->offset.x, dev->offset.y, dev->offset.z);
        }
    }
    else
    {
        dev->onCaliRgbCnt++;
        if (dev->onCaliRgbCnt >= RGB_CNT_MAX)
        {
            dev->onCaliRgbCnt = 0;
        }
        if (dev->onCaliRgbCnt / (RGB_CNT_MAX / 3) == 0)
            rbg_led_set(255, 0, 0);
        else if (dev->onCaliRgbCnt / (RGB_CNT_MAX / 3) == 1)
            rbg_led_set(0, 255, 0);
        else
            rbg_led_set(0, 0, 255);
    }
    return 0;
}

// digitalOsc_Head();
// digitalOsc_int16(dev->adc.y);
// digitalOsc_int16(dev->variance.variance);
// digitalOsc_int16(dev->variance.average);
// digitalOsc_End();
// rt_kprintf("calibrating Z+:%d ", dev->adc.z);
// rt_kprintf("variance: %d ", dev->variance.variance);
// rt_kprintf("average: %d \n", dev->variance.average);
int zino_acc_update(struct acc_device *dev)
{
    int ret = dev->get_raw_data(dev);
    if (dev->calibrated_face != 0x3f)
    {
        dev->perform_6_face_calibration(dev);
    }
    dev->value.x = (float)(dev->adc.x - dev->offset.x) * dev->scale_factor;
    dev->value.y = (float)(dev->adc.y - dev->offset.y) * dev->scale_factor;
    dev->value.z = (float)(dev->adc.z - dev->offset.z) * dev->scale_factor;
    return ret;
}
struct acc_device zAccDev = {
    .init = zino_acc_init,
    .get_raw_data = zino_get_raw_data,
    .perform_6_face_calibration = zino_acc_perform_6_face_calibration,
    .update = zino_acc_update,
};
void acc_6face_calibration_key_handle(uint8_t times)
{
    if (times == 6)
    {
        if (zAccDev.calibrated_face == 0x3f)
        {
            zAccDev.calibrated_face = 0;
            LOG_D("start 6 face calibration.");
        }
        else
        {
            zAccDev.calibrated_face = 0x3f;
            LOG_D("stop 6 face calibration.");
        }
    }
}

void zino_accel_task(void *pvParameters)
{
    struct acc_device *dev = &zAccDev;
    int ret = 0;
    while (1)
    {
        if (dev->init(dev) == RT_EOK)
        {
            break;
        }
        else
        {
            ret++;
            LOG_E("Acceleration init try %d...", ret);
            rt_thread_mdelay(100);
        }
        if (ret >= 5)
        {
            LOG_E("Acceleration init failed, exit");
            return;
        }
    }

    while (1)
    {
        ret = dev->get_raw_data(dev);
        if (dev->calibrated_face != 0x3f)
        {
            dev->perform_6_face_calibration(dev);
        }
        dev->value.x = (float)(dev->adc.x - dev->offset.x) * dev->scale_factor;
        dev->value.y = (float)(dev->adc.y - dev->offset.y) * dev->scale_factor;
        dev->value.z = (float)(dev->adc.z - dev->offset.z) * dev->scale_factor;
        // LOG_D("Acceleration: %d, %d, %d", dev->adc.x, dev->adc.y, dev->adc.z);
        rt_thread_mdelay(10);
    }
}
int zino_accel_init(void)
{
    rt_thread_t tid = rt_thread_create("zino_accel",
                                       zino_accel_task,
                                       RT_NULL,
                                       2048,
                                       20,
                                       10);

    if (tid != RT_NULL)
    {
        rt_thread_startup(tid);
        LOG_D("zino_accel_init");
    }
    else
    {
        LOG_E("zino_accel_init failed");
    }
    return RT_EOK;
}
// ZINO_APP_EXPORT(zino_accel_init);
